/*
 * Menu.cpp
 *
 *  Created on: Dec 12, 2011
 *      Author: Michael Anderson
 *      Part of Team 955 Battery Cart project
 *      compiled on Eclipse with Arduino libraries and avr-gcc
 */

#include "Menu.h"

Menu::Menu(Display*(&disps)[NUM_DISPLAYS], MyKeypad &keyp, EventManager &eman, Battery*(&batts)[NUM_CHARGING_CIRCUITS], Charger*(&chargs)[NUM_CHARGING_CIRCUITS], Conditioner*(&conds)[NUM_CHARGING_CIRCUITS / 2]) {
    keypd = &keyp;
    eManager = &eman;
    displays = disps;
    batteries = batts;
    chargers = chargs;
    conditioners = conds;
    state = MENU_HOME;
    lastState = MENU_HOME;
    promptAction = MENU_ACTION_NONE;

    eManager->setEventListener(this);
    for (uint8_t i = 0; i < NUM_CHARGING_CIRCUITS; i++) {
        clearQueue(i);
    }
    clearResults();
    debugMenuPage = 0;
    resultsMenuPage = 0;
    showHomeMenu();

    //Bah! I hate these things!
    //eManager->setChargerEventListener(&Menu::updateStatus);
    //eManager->setConditionerEventListener(&Menu::updateStatus);
}

void Menu::run() {
    loopStartTime = millis();

    char key = keypd->getKey();

    if (ENABLE_SERIAL_MENUS && key == 'd') { //serial debug
        char str[128];
        Serial.println((char*)"");
        sprintf(str, "ST:  B1% i  CHA1 %i  CN1 %i", batteries[0]->getState(), chargers[0]->getState(), conditioners[0]->getState());
        Serial.println(str);
        sprintf(str, "ST:  B2% i  CHA2 %i  CN2 %i", batteries[1]->getState(), chargers[1]->getState(), conditioners[1]->getState());
        Serial.println(str);
        sprintf(str, "ST:  B3% i  CHA3 %i  MEN %i", batteries[2]->getState(), chargers[2]->getState(), state);
        Serial.println(str);
        sprintf(str, "ST:  B4% i  CHA4 %i", batteries[3]->getState(), chargers[3]->getState());
        Serial.println(str);
        Serial.println((char*)"");
    }

    switch (state) { //processes the keypad input based on the current menu
        case MENU_HOME: {
            uint8_t num = getCharInt(key);

            if (num > 0 && num <= NUM_CHARGING_CIRCUITS) {
                showBatteryMenu(num - 1); //Convert from base 1 to base 0
            }
            else if (key == KEY_OPEN_WATT_MENU) {
                showResultsMenu(RESULTS_PAGE_WATTHOURS);
            }
            else if (ENABLE_DEBUG_MENU && key == KEY_SUBMENU_FORWARDS) {
                showDebugMenu(DEBUG_PAGE_BATTERIES);
            }
            break;
        }

        case MENU_BATTERY_STATUS: {
            bool queueEmpty = queueIsEmpty(curBattID);
            if (batteries[curBattID]->getState() == BATTERY_IDLE && queueEmpty) {
                if (key == KEY_START_CHARGE) {
                    connectCharger(curBattID);
                }
                else if (key == KEY_START_COND) {
                    lastState = state;
                    showConditionTypeMenu();
                }
            }
            else if (key == KEY_MENU_CONFIRM) {
                lastState = state;
                if (queueEmpty || !batteries[curBattID]->getState() == BATTERY_IDLE) {
                    char* str[] = { (char*)"Are you sure that", (char*)"you wish to cancel", (char*)"the current action?", (char*)"  *: NO     #: YES" };
                    promptAction = MENU_ACTION_CANCEL_PROCESS;
                    showPromptMenu(str, 4);
                }
                else {
                    char* str[] = { (char*)" Are you sure that", (char*)" you wish to clear", (char*)"     the queue?", (char*)"  *: NO     #: YES" };
                    promptAction = MENU_ACTION_CLEAR_QUEUE;
                    showPromptMenu(str, 4);
                }
            }
            if (key == KEY_MENU_BACK) {
                showHomeMenu();
            }
            else if (key == KEY_OPEN_QUEUE) {
                showQueueMenu(curBattID);
            }
            break;
        }

        case MENU_PROMPT: {
            if (key == KEY_MENU_CONFIRM) {
                if (promptAction == MENU_ACTION_CANCEL_PROCESS) {
                    battery_state battState = batteries[curBattID]->getState();
                    switch (battState) {
                        case BATTERY_CHARGING: {
                            debugln("Canceling charge");
                            disconnectCharger(curBattID);
                            break;
                        }

                        case BATTERY_CONDITIONING: {
                            disconnectConditioner(curBattID);
                            break;
                        }

                        case BATTERY_TESTING: {
                            disconnectConditioner(curBattID);
                            break;
                        }

                        case BATTERY_CONNECTED_TO_CONDITIONER: {
                            disconnectConditioner(curBattID);
                            break;
                        }

                        case BATTERY_CONNECTED_TO_CHARGER: //Currently not possible
                            break;

                        case BATTERY_IDLE: //Should never happen
                            break;
                    }

                    //Return to the last menu
                    state = lastState;
                    refreshMenu();
                }
                else if (promptAction == MENU_ACTION_CLEAR_QUEUE) {
                    clearQueue(curBattID);

                    //Return to the last menu
                    state = lastState;
                    refreshMenu();
                }
                else if (promptAction == MENU_ACTION_CLEAR_RESULTS) {
                    clearResults();

                    //Return to the last menu
                    state = lastState;
                    refreshMenu();
                }
            }
            else if (key == KEY_MENU_BACK) {
                //Return to the last menu
                state = lastState;
                refreshMenu();
            }
            break;
        }
        case MENU_CONDITION_TYPE: {
            if (key == KEY_MENU_BACK) {
                state = lastState;
                refreshMenu();
                break;
            }
            switch(lastState) {
                case MENU_QUEUE: {
                    if (key == KEY_START_CHARGE) {
                        queueAction(curBattID, QUEUE_ACTION_CONDITION_PULSE);
                        state = lastState;
                        refreshMenu();
                    }
                    else if (key == KEY_START_COND) {
                        queueAction(curBattID, QUEUE_ACTION_CONDITION_LOW);
                        state = lastState;
                        refreshMenu();
                    }
                    else if (key == KEY_START_QUICK_TEST) {
                        queueAction(curBattID, QUEUE_ACTION_TEST_QUICK);
                        state = lastState;
                        refreshMenu();
                    }
                    else if (key == KEY_START_HIGH_RATE_TEST) {
                        queueAction(curBattID, QUEUE_ACTION_TEST_HIGH_RATE);
                        state = lastState;
                        refreshMenu();
                    }
                    break;
                }
                case MENU_BATTERY_STATUS: {
                    if (key == KEY_START_CHARGE) {
                        connectConditioner(curBattID, DISCHARGE_CONDITION_PULSE);
                        state = lastState;
                        refreshMenu();
                    }
                    else if (key == KEY_START_COND) {
                        connectConditioner(curBattID, DISCHARGE_CONDITION_LOW);
                        state = lastState;
                        refreshMenu();
                    }
                    else if (key == KEY_START_QUICK_TEST) {
                        connectConditioner(curBattID, DISCHARGE_TEST_QUICK);
                        state = lastState;
                        refreshMenu();
                    }
                    else if (key == KEY_START_HIGH_RATE_TEST) {
                        connectConditioner(curBattID, DISCHARGE_TEST_HIGH_RATE, false);
                        state = lastState;
                        refreshMenu();
                    }
                    break;
                }
                default: //These should be the only two menus calling showConditionTypeMenu()
                    break;
            }
            break;
        }
        case MENU_QUEUE: {
            if (key == KEY_MENU_BACK) {
                showBatteryMenu(curBattID);
            }
            else if (key == KEY_MENU_CONFIRM) {
                lastState = state;
                char* str[] = { (char*)" Are you sure that", (char*)" you wish to clear", (char*)"     the queue?", (char*)"  *: NO     #: YES" };
                promptAction = MENU_ACTION_CLEAR_QUEUE;
                showPromptMenu(str, 4);
            }
            else if (key == KEY_START_CHARGE) {
                queueAction(curBattID, QUEUE_ACTION_CHARGE);
                refreshMenu();
            }
            else if (key == KEY_START_COND) {
                lastState = state;
                showConditionTypeMenu();
            }
            else if (key == KEY_START_QUICK_TEST) {
                queueAction(curBattID, QUEUE_ACTION_TEST_QUICK);
                refreshMenu();
            }
            break;
        }
        case MENU_RESULTS: {
            if (key == KEY_MENU_BACK) {
                showHomeMenu();
            }
            else if (key == KEY_SUBMENU_FORWARDS) {
                if (resultsMenuPage < RESULTS_MENU_NUM_PAGES - 1) {
                    resultsMenuPage++;
                    showResultsMenu(resultsMenuPage);
                }
            }
            else if (key == KEY_SUBMENU_BACK) {
                if (resultsMenuPage > 0) {
                    resultsMenuPage--;
                    showResultsMenu(resultsMenuPage);
                }
            }
            else if (key == KEY_MENU_CONFIRM) {
                lastState = state;
                char* str[] = { (char*)" Are you sure that", (char*)" you wish to clear", (char*)"    all results?", (char*)"  *: NO     #: YES" };
                promptAction = MENU_ACTION_CLEAR_RESULTS;
                showPromptMenu(str, 4);
            }
            else if (resultsMenuPage == RESULTS_PAGE_WATTHOURS && getMillisSince(wattHourUpdateTimer, millis()) > WATTHOUR_MENU_REFRESH_DELAY) {
                for (uint8_t i = 0; i < NUM_CONDITIONERS; i++) {
                    if (conditioners[i]->getState() > CONDITIONER_CONNECTED) {
                        refreshMenu();
                        wattHourUpdateTimer = millis();
                        break;
                    }
                }
            }
            break;
        }
        case MENU_DEBUG: {
            if (key == KEY_MENU_BACK) {
                debugMenuPage = 0;
                showHomeMenu();
            }
            else if (key == KEY_SUBMENU_FORWARDS) {
                if (debugMenuPage < DEBUG_MENU_NUM_PAGES - 1) {
                    debugMenuPage++;
                    showDebugMenu(debugMenuPage);
                }
            }
            else if (key == KEY_SUBMENU_BACK) {
                if (debugMenuPage > 0) {
                    debugMenuPage--;
                    showDebugMenu(debugMenuPage);
                }
            }
            else if ((debugMenuPage == DEBUG_PAGE_CURRENTS || debugMenuPage == DEBUG_PAGE_VOLTAGES || debugMenuPage == DEBUG_PAGE_MISC) && getMillisSince(debugMenuRefreshTimer, millis()) >= DEBUG_MENU_REFRESH_DELAY) {
                refreshMenu();
            }
            break;
        }
    }

    displays[DISPLAY_PRIMARY]->run();
    for (uint8_t n = 0; n < NUM_CONDITIONERS; n++) {
        conditioners[n]->run();
        if (conditioners[n]->getState() > CONDITIONER_CONNECTED) {
            wattHourReadings[conditioners[n]->getConnectedBattID()] = conditioners[n]->getLastWattHours();
        }
    }
    for (uint8_t n = 0; n < NUM_CHARGING_CIRCUITS; n++) {
        chargers[n]->run();
    }
    lastLoopTime = getMillisSince(loopStartTime, millis());
}

void Menu::showHomeMenu() {
    state = MENU_HOME;
    displays[DISPLAY_PRIMARY]->clear(false);
    displays[DISPLAY_PRIMARY]->setFullScreen(false);
    displays[DISPLAY_PRIMARY]->setLine(0, (char*)"Press 1-4 to access");
    displays[DISPLAY_PRIMARY]->setLine(1, (char*)"a battery's controls");
    updateStatus();
}

void Menu::showBatteryMenu(uint8_t battID) { //Base 0 ID
    state = MENU_BATTERY_STATUS;
    curBattID = battID;
    displays[DISPLAY_PRIMARY]->setFullScreen(true);
    displays[DISPLAY_PRIMARY]->clear(false);

    switch (batteries[curBattID]->getState()) {
        case BATTERY_CHARGING: {
            char str[DISPLAY_COLS + 1];
            sprintf(str, "Batt %i: Charging", curBattID + 1);
            displays[DISPLAY_PRIMARY]->setLine(0, str);
            displays[DISPLAY_PRIMARY]->setLine(1, (char*)"0) Queue Menu");
            displays[DISPLAY_PRIMARY]->setLine(3, (char*)" Press # to cancel");
            break;
        }

        case BATTERY_CONDITIONING: {
            char str[DISPLAY_COLS + 1];
            sprintf(str, "Batt %i: Conditioning", curBattID + 1);
            displays[DISPLAY_PRIMARY]->setLine(0, str);
            displays[DISPLAY_PRIMARY]->setLine(1, (char*)"0) Queue Menu");
            displays[DISPLAY_PRIMARY]->setLine(3, (char*)" Press # to cancel");
            break;
        }

        case BATTERY_CONNECTED_TO_CHARGER: //Never happens
            break;

        case BATTERY_CONNECTED_TO_CONDITIONER: {
            char str[DISPLAY_COLS + 1];
            sprintf(str, "Batt %i: Conn to cond", curBattID + 1);
            displays[DISPLAY_PRIMARY]->setLine(0, str);
            displays[DISPLAY_PRIMARY]->setLine(1, (char*)"0) Queue Menu");
            displays[DISPLAY_PRIMARY]->setLine(3, (char*)" Press # to cancel");
            break;
        }

        case BATTERY_TESTING: {
            char str[DISPLAY_COLS + 1];
            sprintf(str, "Batt %i: Testing", curBattID + 1);
            displays[DISPLAY_PRIMARY]->setLine(0, str);
            displays[DISPLAY_PRIMARY]->setLine(1, (char*)"0) Queue Menu");
            displays[DISPLAY_PRIMARY]->setLine(3, (char*)" Press # to cancel");
            break;
        }

        case BATTERY_IDLE: {
            if (queueIsEmpty(curBattID)) {
                char str[DISPLAY_COLS + 1];
                sprintf(str, "Batt %i: Idle", curBattID + 1);
                displays[DISPLAY_PRIMARY]->setLine(0, str);
                displays[DISPLAY_PRIMARY]->setLine(1, (char*)"0) Queue Menu");
                displays[DISPLAY_PRIMARY]->setLine(2, (char*)"1) Charge");
                displays[DISPLAY_PRIMARY]->setLine(3, (char*)"2) Condition");
                break;
            }
            else {
                char str[DISPLAY_COLS + 1];
                sprintf(str, "Batt %i: Waiting...", curBattID + 1);
                displays[DISPLAY_PRIMARY]->setLine(0, str);
                displays[DISPLAY_PRIMARY]->setLine(1, (char*)"0) Queue Menu");
                displays[DISPLAY_PRIMARY]->setLine(3, (char*)" Press # to cancel");
                break;
            }
            break;
        }
    }
}

void Menu::showDebugMenu(uint8_t page) {
    state = MENU_DEBUG;
    debugMenuRefreshTimer = millis();
    displays[DISPLAY_PRIMARY]->setFullScreen(true);
    displays[DISPLAY_PRIMARY]->clear(false);
    char line[DISPLAY_COLS + 1];
    switch (page) {
        case DEBUG_PAGE_BATTERIES: {
            displays[DISPLAY_PRIMARY]->setLine(0, (char*)"BATTERIES");
            sprintf(line, "Batt 1: %i  Batt 2: %i", batteries[0]->getState(), batteries[1]->getState());
            displays[DISPLAY_PRIMARY]->setLine(1, line);
            sprintf(line, "Batt 3: %i  Batt 4: %i", batteries[2]->getState(), batteries[3]->getState());
            displays[DISPLAY_PRIMARY]->setLine(2, line);
            break;
        }
        case DEBUG_PAGE_CHARGERS: {
            displays[DISPLAY_PRIMARY]->setLine(0, (char*)"CHARGERS");
            sprintf(line, "Chrg 1: %i  Chrg 2: %i", chargers[0]->getState(), chargers[1]->getState());
            displays[DISPLAY_PRIMARY]->setLine(1, line);
            sprintf(line, "Chrg 3: %i  Chrg 4: %i", chargers[2]->getState(), chargers[3]->getState());
            displays[DISPLAY_PRIMARY]->setLine(2, line);
            break;
        }
        case DEBUG_PAGE_CONDITIONERS: {
            displays[DISPLAY_PRIMARY]->setLine(0, (char*)"COND STATES");
            sprintf(line, "Cond 1: %i  Cond 2: %i", conditioners[0]->getState(), conditioners[1]->getState());
            displays[DISPLAY_PRIMARY]->setLine(1, line);
            break;
        }
        case DEBUG_PAGE_VOLTAGES: {
            displays[DISPLAY_PRIMARY]->setLine(0, (char*)"COND VOLTAGES");
            char tmp[DISPLAY_COLS - 8];
            Utils::doubleToCharArray(tmp, 12, conditioners[0]->getVoltage(), 3);
            sprintf(line, "Cond 1: %s", tmp);
            displays[DISPLAY_PRIMARY]->setLine(1, line);
            Utils::doubleToCharArray(tmp, 12, conditioners[1]->getVoltage(), 3);
            sprintf(line, "Cond 2: %s", tmp);
            displays[DISPLAY_PRIMARY]->setLine(2, line);
            sprintf(line, "RA1: %i  RA2: %i", conditioners[0]->getRawVoltage(), conditioners[1]->getRawVoltage());
            displays[DISPLAY_PRIMARY]->setLine(3, line);
            break;
        }
        case DEBUG_PAGE_CURRENTS: {
            displays[DISPLAY_PRIMARY]->setLine(0, (char*)"COND CURRENTS");
            char tmp[DISPLAY_COLS - 8];
            Utils::doubleToCharArray(tmp, 12, conditioners[0]->getCurrent(), 3);
            sprintf(line, "Cond 1: %s", tmp);
            displays[DISPLAY_PRIMARY]->setLine(1, line);
            Utils::doubleToCharArray(tmp, 12, conditioners[1]->getCurrent(), 3);
            sprintf(line, "Cond 2: %s", tmp);
            displays[DISPLAY_PRIMARY]->setLine(2, line);
            sprintf(line, "RA1: %i  RA2: %i", conditioners[0]->getRawCurrent(), conditioners[1]->getRawCurrent());
            displays[DISPLAY_PRIMARY]->setLine(3, line);
            break;
        }
        case DEBUG_PAGE_MISC: {
            displays[DISPLAY_PRIMARY]->setLine(0, (char*)"MISCELLANEOUS");
            char tmp[DISPLAY_COLS + 1];
            sprintf(tmp, "Loop millis: %i", lastLoopTime);
            displays[DISPLAY_PRIMARY]->setLine(1, tmp);
            sprintf(tmp, "Free mem: %i bytes", freeMemory());
            displays[DISPLAY_PRIMARY]->setLine(2, tmp);
            break;
        }
        default: {
            displays[DISPLAY_PRIMARY]->setLine(1, (char*)"  Oops! This page");
            displays[DISPLAY_PRIMARY]->setLine(2, (char*)"  does not exist!");
            break;
        }
    }
    //sprintf(line, "Page: %i", (uint8_t)page);
    //displays[DISPLAY_PRIMARY]->setLine(3, line);
}

void Menu::showQueueMenu(uint8_t battID) {
    displays[DISPLAY_PRIMARY]->setFullScreen(true);
    displays[DISPLAY_PRIMARY]->clear(false);
    state = MENU_QUEUE;
    uint8_t i = actionQueueStart[battID];
    char tmp[DISPLAY_COLS + 1];
    uint8_t t = 1;

    while (t < DISPLAY_ROWS && t <= QUEUE_MAX_DEPTH) {
        if (actionQueue[battID][i] == QUEUE_ACTION_NONE) {
            break;
        }
        else {
            switch (actionQueue[battID][i]) {
                case QUEUE_ACTION_CHARGE: {
                    if (t == 1) {
                        displays[DISPLAY_PRIMARY]->setLine(0, (char*)"1) Charge (next)", false, false);
                    }
                    else {
                        sprintf(tmp, "%i) Charge", t);
                        displays[DISPLAY_PRIMARY]->setLine(t - 1, tmp, false, false);
                    }
                    break;
                }
                case QUEUE_ACTION_CONDITION_PULSE: {
                    if (t == 1) {
                        displays[DISPLAY_PRIMARY]->setLine(0, (char*)"1) Pulse cond (next)", false, false);
                    }
                    else {
                        sprintf(tmp, "%i) Pulse cond", t);
                        displays[DISPLAY_PRIMARY]->setLine(t - 1, tmp, false, false);
                    }
                    break;
                }
                case QUEUE_ACTION_CONDITION_LOW: {
                    if (t == 1) {
                        displays[DISPLAY_PRIMARY]->setLine(0, (char*)"1) Low cond (next)", false, false);
                    }
                    else {
                        sprintf(tmp, "%i) Low cond", t);
                        displays[DISPLAY_PRIMARY]->setLine(t - 1, tmp, false, false);
                    }
                    break;
                }
                case QUEUE_ACTION_TEST_QUICK: {
                    if (t == 1) {
                        displays[DISPLAY_PRIMARY]->setLine(0, (char*)"1) Quick Test (next)", false, false);
                    }
                    else {
                        sprintf(tmp, "%i) Quick Test", t);
                        displays[DISPLAY_PRIMARY]->setLine(t - 1, tmp, false, false);
                    }
                    break;
                }
                case QUEUE_ACTION_TEST_HIGH_RATE: {
                    if (t == 1) {
                        displays[DISPLAY_PRIMARY]->setLine(0, (char*)"1) High Rate Test", false, false);
                    }
                    else {
                        sprintf(tmp, "%i) High Rate Test", t);
                        displays[DISPLAY_PRIMARY]->setLine(t - 1, tmp, false, false);
                    }
                    break;
                }
                case QUEUE_ACTION_NONE: {
                    break;
                }
            }
        }
        t++;
        i = (i + 1) % QUEUE_MAX_DEPTH;
    }
    if (t == 1) {
        displays[DISPLAY_PRIMARY]->setLine(1, (char*)"No queued actions!", false, false);
        displays[DISPLAY_PRIMARY]->setLine(2, (char*)"Press 1 or 2 to add!", false, false);
    }
}

void Menu::showPromptMenu(char* str[], uint8_t len) {
    state = MENU_PROMPT;
    for (uint8_t i = 0; i < len; i++) {
        displays[DISPLAY_PRIMARY]->setLine(i, str[i]);
    }
}

void Menu::showConditionTypeMenu() {
    state = MENU_CONDITION_TYPE;
    displays[DISPLAY_PRIMARY]->setLine(0, (char*)"1) Multi-stage cond.");
    displays[DISPLAY_PRIMARY]->setLine(1, (char*)"2) Low-rate cond.");
    displays[DISPLAY_PRIMARY]->setLine(2, (char*)"3) Quick test");
    displays[DISPLAY_PRIMARY]->setLine(3, (char*)"4) High Rate Test");
}

void Menu::showResultsMenu(uint8_t page) {
    displays[DISPLAY_PRIMARY]->setFullScreen(true);
    displays[DISPLAY_PRIMARY]->clear(false);
    state = MENU_RESULTS;
    debugMenuPage = page;

    switch (page) {
        case RESULTS_PAGE_TEST_QUICK: {
            uint8_t l = 0;
            for (uint8_t i = 0; i < NUM_CHARGING_CIRCUITS; i++) { //Loop through each battery
                if (i % WATTHOUR_MENU_DATA_COLS == 0) { //Is this the start of a new line?
                    char line[DISPLAY_COLS + 1];
                    line[0] = '\0';
                    char line2[DISPLAY_COLS + 1];
                    line2[0] = '\0';
                    char buffer[DISPLAY_COLS + 1];
                    buffer[0] = '\0';

                    bool print = false;
                    for (uint8_t j = 0; j < WATTHOUR_MENU_DATA_COLS; j++) { //Loop through each battery that fits on the line
                        if (i + j < NUM_CHARGING_CIRCUITS) {
                            print = true;
                            uint8_t entryWidth = (uint8_t)(DISPLAY_COLS / WATTHOUR_MENU_DATA_COLS);
                            bool entryEnded = false;
                            char entry[entryWidth + 1];
                            entry[0] = '\0';
                            switch(quickTestResults[i + j]) {
                                case QUICK_TEST_STATUS_NOT_TESTED: {
                                    strcat(entry, (char*)"Untested");
                                    break;
                                }
                                case QUICK_TEST_STATUS_TESTING: {
                                    strcat(entry, (char*)"Testing");
                                    break;
                                }
                                case QUICK_TEST_STATUS_PASS: {
                                    strcat(entry, (char*)"Passed!");
                                    break;
                                }
                                case QUICK_TEST_STATUS_NO_PASS: {
                                    strcat(entry, (char*)"Failed!");
                                    break;
                                }
                                case QUICK_TEST_STATUS_ERROR: {
                                    strcat(entry, (char*)"ERROR");
                                    break;
                                }
                            }
                            for (uint8_t k = 0; k < entryWidth; k++) { //Copy over the result
                                if (j == WATTHOUR_MENU_DATA_COLS - 1 && k == entryWidth - 1) {
                                    line[j * entryWidth + k] = '\0';
                                    break;
                                }
                                else if (entryEnded || entry[k] == '\0') {
                                    entryEnded = true;
                                    line[j * entryWidth + k] = ' ';
                                }
                                else if (k <= entryWidth - WATTHOUR_MENU_ENTRY_SPACING - 1)
                                    line[j * entryWidth + k] = entry[k];
                                else
                                    line[j * entryWidth + k] = ' ';
                            }
                            debugln((char*)"Copied line:");
                            debugln(line);
                            if (j == 0) {
                                sprintf(line2, "Batt. %i", i + j + 1);
                            }
                            else {
                                sprintf(buffer, "%s   Batt. %i", line2, i + j + 1);
                                uint8_t m = 0;
                                while (buffer[m]) {
                                    line2[m] = buffer[m];
                                    m++;
                                }
                                line2[m] = '\0';
                            }
                            debugln((char*)"Label line:");
                            debugln(line2);
                        }
                    }
                    if (print) {
                        debugln((char*)"Printing label line:");
                        displays[DISPLAY_PRIMARY]->setLine(l, line2, false, true);
                        l++;
                        debugln((char*)"Printing status line:");
                        displays[DISPLAY_PRIMARY]->setLine(l, line, false, true);
                        l++;
                    }
                }
                if (l == DISPLAY_ROWS - 1)
                    break;
            }
            break;
        }
        case RESULTS_PAGE_TEST_HIGH_RATE: {
            wattHourUpdateTimer = millis();
            uint8_t l = 0;
            for (uint8_t i = 0; i < NUM_CHARGING_CIRCUITS; i++) { //Loop through each battery
                if (i % WATTHOUR_MENU_DATA_COLS == 0) { //Is this the start of a new line?
                    char line[DISPLAY_COLS + 1];
                    line[0] = '\0';
                    char line2[DISPLAY_COLS + 1];
                    line2[0] = '\0';
                    char buffer[DISPLAY_COLS + 1];
                    buffer[0] = '\0';

                    bool print = false;
                    for (uint8_t j = 0; j < WATTHOUR_MENU_DATA_COLS; j++) { //Loop through each battery that fits on the line
                        if (i + j < NUM_CHARGING_CIRCUITS) {
                            print = true;
                            uint8_t entryWidth = (uint8_t)(DISPLAY_COLS / WATTHOUR_MENU_DATA_COLS);
                            bool entryEnded = false;
                            char entry[entryWidth + 1];
                            Utils::doubleToCharArray(entry, entryWidth, highRateTestResults[i + j], WATTHOUR_MENU_PRECISION);
                            debugln((char*)"Formatdouble result:");
                            debugln(entry);
                            for (uint8_t k = 0; k < entryWidth; k++) { //Copy over the watt-hour reading
                                if (j == WATTHOUR_MENU_DATA_COLS - 1 && k == entryWidth - 1) {
                                    line[j * entryWidth + k] = '\0';
                                    break;
                                }
                                else if (entryEnded || entry[k] == '\0') {
                                    entryEnded = true;
                                    line[j * entryWidth + k] = ' ';
                                }
                                else if (k <= entryWidth - WATTHOUR_MENU_ENTRY_SPACING - 1)
                                    line[j * entryWidth + k] = entry[k];
                                else
                                    line[j * entryWidth + k] = ' ';
                            }
                            debugln((char*)"Copied line:");
                            debugln(line);
                            if (j == 0) {
                                sprintf(line2, "Batt. %i", i + j + 1);
                            }
                            else {
                                sprintf(buffer, "%s   Batt. %i", line2, i + j + 1);
                                uint8_t m = 0;
                                while (buffer[m]) {
                                    line2[m] = buffer[m];
                                    m++;
                                }
                                line2[m] = '\0';
                            }
                            debugln((char*)"Label line:");
                            debugln(line2);
                        }
                    }
                    if (print) {
                        debugln((char*)"Printing label line:");
                        displays[DISPLAY_PRIMARY]->setLine(l, line2, false, true);
                        l++;
                        debugln((char*)"Printing status line:");
                        displays[DISPLAY_PRIMARY]->setLine(l, line, false, true);
                        l++;
                    }
                }
                if (l == DISPLAY_ROWS - 1)
                    break;
            }
            break;
        }
        case RESULTS_PAGE_WATTHOURS: {
            wattHourUpdateTimer = millis();
            uint8_t l = 0;
            for (uint8_t i = 0; i < NUM_CHARGING_CIRCUITS; i++) { //Loop through each battery
                if (i % WATTHOUR_MENU_DATA_COLS == 0) { //Is this the start of a new line?
                    char line[DISPLAY_COLS + 1];
                    line[0] = '\0';
                    char line2[DISPLAY_COLS + 1];
                    line2[0] = '\0';
                    char buffer[DISPLAY_COLS + 1];
                    buffer[0] = '\0';

                    bool print = false;
                    for (uint8_t j = 0; j < WATTHOUR_MENU_DATA_COLS; j++) { //Loop through each battery that fits on the line
                        if (i + j < NUM_CHARGING_CIRCUITS) {
                            print = true;
                            uint8_t entryWidth = (uint8_t)(DISPLAY_COLS / WATTHOUR_MENU_DATA_COLS);
                            bool entryEnded = false;
                            char entry[entryWidth + 1];
                            Utils::doubleToCharArray(entry, entryWidth, wattHourReadings[i + j], WATTHOUR_MENU_PRECISION);
                            debugln((char*)"Formatdouble result:");
                            debugln(entry);
                            for (uint8_t k = 0; k < entryWidth; k++) { //Copy over the watt-hour reading
                                if (j == WATTHOUR_MENU_DATA_COLS - 1 && k == entryWidth - 1) {
                                    line[j * entryWidth + k] = '\0';
                                    break;
                                }
                                else if (entryEnded || entry[k] == '\0') {
                                    entryEnded = true;
                                    line[j * entryWidth + k] = ' ';
                                }
                                else if (k <= entryWidth - WATTHOUR_MENU_ENTRY_SPACING - 1)
                                    line[j * entryWidth + k] = entry[k];
                                else
                                    line[j * entryWidth + k] = ' ';
                            }
                            debugln((char*)"Copied line:");
                            debugln(line);
                            if (j == 0) {
                                sprintf(line2, "Batt. %i", i + j + 1);
                            }
                            else {
                                sprintf(buffer, "%s   Batt. %i", line2, i + j + 1);
                                uint8_t m = 0;
                                while (buffer[m]) {
                                    line2[m] = buffer[m];
                                    m++;
                                }
                                line2[m] = '\0';
                            }
                            debugln((char*)"Label line:");
                            debugln(line2);
                        }
                    }
                    if (print) {
                        debugln((char*)"Printing label line:");
                        displays[DISPLAY_PRIMARY]->setLine(l, line2, false, true);
                        l++;
                        debugln((char*)"Printing status line:");
                        displays[DISPLAY_PRIMARY]->setLine(l, line, false, true);
                        l++;
                    }
                }
                if (l == DISPLAY_ROWS - 1)
                    break;
            }
            debugln((char*)"All done!");\
            break;
        }
        default: {
            displays[DISPLAY_PRIMARY]->setLine(1, (char*)"  Oops! This page");
            displays[DISPLAY_PRIMARY]->setLine(2, (char*)"  does not exist!");
            break;
        }
    }
}

void Menu::updateStatus() {
    char statuses[NUM_CHARGING_CIRCUITS][DISPLAY_COLS + 1];

    //Strcat looks for the first nullchar it finds then appends after it.
    //Our arrays don't come standard with nullchars at the beginning, so we
    //have to add them or it will include a bunch of random data (and usually crash)
    //(This was the source of quite a lot of time spent debugging)
    for (uint8_t i = 0; i < NUM_CHARGING_CIRCUITS; i++) {
        statuses[i][0] = '\0';
    }

    battery_state battState;
    for (uint8_t i = 0; i < NUM_CHARGING_CIRCUITS; i++) {
        battState = batteries[i]->getState();
        switch (battState) {
            case BATTERY_CHARGING: {
                strcat(statuses[i], (char*)"Charging");
                break;
            }

            case BATTERY_CONNECTED_TO_CHARGER: {
                strcat(statuses[i], (char*)"Charging");
                break;
            }

            case BATTERY_CONDITIONING: {
                strcat(statuses[i], (char*)"Cond.");
                break;
            }

            case BATTERY_CONNECTED_TO_CONDITIONER: {
                strcat(statuses[i], (char*)"Cond. Connected");
                break;
            }

            case BATTERY_TESTING: {
                strcat(statuses[i], (char*)"Testing");
                break;
            }

            case BATTERY_IDLE: {
                if (queueIsEmpty(i))
                    strcat(statuses[i], (char*)"Idle");
                else
                    strcat(statuses[i], (char*)"Waiting");
                break;
            }
        }

    }
    char str[MAX_TICKER_LEN];
    sprintf(str, "B1: %s, B2: %s, B3: %s, B4: %s,", statuses[0], statuses[1], statuses[2], statuses[3]);
    displays[DISPLAY_PRIMARY]->setLine(3, str, true, true);
}

void Menu::refreshMenu() {
    switch (state) {
        case MENU_BATTERY_STATUS: {
            showBatteryMenu(curBattID);
            break;
        }

        case MENU_HOME: {
            showHomeMenu();
            break;
        }

        case MENU_QUEUE: {
            showQueueMenu(curBattID);
            break;
        }

        case MENU_RESULTS: {
            showResultsMenu(resultsMenuPage);
            break;
        }

        case MENU_PROMPT: //This ought not to happen
            break;

        case MENU_CONDITION_TYPE: //Nor this
            break;

        case MENU_DEBUG:
            showDebugMenu(debugMenuPage);
            break;
    }
}

uint8_t Menu::getCharInt(char k) {
    if (k == NO_KEY)
        return k;
    if (k < '0' || k > '9')
        return NO_KEY;
    return k - '0';
}

void Menu::_chargerEventCallback(uint8_t chargID, charger_event event) {
    char str[32];
    if (ENABLE_SERIAL_DEBUG)
        sprintf(str, "Charger event! ID=%i, event=%i", chargID, event);
    debugln(str);

    switch(event) {
        case EVENT_CHARGER_DISCONNECTED: {
            conditioner_state condState = (chargID > 1) ? conditioners[1]->getState() : conditioners[0]->getState();
            queue_action nextQueue = queueGetNext(chargID);

            if (condState == CONDITIONER_IDLE) {
                if (nextQueue == QUEUE_ACTION_CONDITION_PULSE && conditioners) {
                    connectConditioner(chargID, DISCHARGE_CONDITION_PULSE);
                    advanceQueue(chargID);
                }
                else if (nextQueue == QUEUE_ACTION_TEST_QUICK) {
                    connectConditioner(chargID, DISCHARGE_TEST_QUICK);
                    advanceQueue(chargID);
                }
            }
            if (nextQueue == QUEUE_ACTION_CHARGE) {
                connectCharger(chargID);
                advanceQueue(chargID);
            }
            break;
        }
        default: {
            break;
        }
    }

    refreshMenu();
}

void Menu::_conditionerEventCallback(uint8_t condID, uint8_t battID, conditioner_event event) {
    if (ENABLE_SERIAL_DEBUG) {
        char str[32];
        sprintf(str, "Cond event! ID=%i, event=%i", condID, event);
        debugln(str);
    }

    if (event == EVENT_CONDITIONER_END) {
        queue_action nextQueue = queueGetNext(battID);
        uint8_t pairedBattery = 0;

        switch (battID) {
            case 0: {
                pairedBattery = 1;
                break;
            }
            case 1: {
                pairedBattery = 0;
                break;
            }
            case 2: {
                pairedBattery = 3;
                break;
            }
            case 3: {
                pairedBattery = 2;
                break;
            }
        }

        if (queueGetNext(pairedBattery) == QUEUE_ACTION_CONDITION_PULSE) {
            connectConditioner(pairedBattery, DISCHARGE_CONDITION_PULSE);
            advanceQueue(pairedBattery);
        }
        else if (queueGetNext(pairedBattery) == QUEUE_ACTION_TEST_QUICK) {
            connectConditioner(pairedBattery, DISCHARGE_TEST_QUICK);
            advanceQueue(pairedBattery);
        }
        else if (queueGetNext(pairedBattery) == QUEUE_ACTION_TEST_HIGH_RATE) {
            connectConditioner(pairedBattery, DISCHARGE_TEST_HIGH_RATE, false);
            advanceQueue(pairedBattery);
        }
        else if (nextQueue == QUEUE_ACTION_CONDITION_PULSE) {
            connectConditioner(battID, DISCHARGE_CONDITION_PULSE);
            advanceQueue(battID);
        }
        else if (nextQueue == QUEUE_ACTION_TEST_QUICK) {
            connectConditioner(battID, DISCHARGE_TEST_QUICK);
            advanceQueue(battID);
        }
        else if (nextQueue == QUEUE_ACTION_TEST_HIGH_RATE) {
            connectConditioner(battID, DISCHARGE_TEST_HIGH_RATE, true);
            advanceQueue(battID);
        }
        if (nextQueue == QUEUE_ACTION_CHARGE) {
            connectCharger(battID);
            advanceQueue(battID);
        }
        highRateTestResults[battID] = conditioners[condID]->getHighRateTestResult();
    }
    else if (event == EVENT_CONDITIONER_HALT) {
        uint8_t pairedBattery = 0;

        switch (battID) {
            case 0: {
                pairedBattery = 1;
                break;
            }
            case 1: {
                pairedBattery = 0;
                break;
            }
            case 2: {
                pairedBattery = 3;
                break;
            }
            case 3: {
                pairedBattery = 2;
                break;
            }
        }
        if (queueGetNext(pairedBattery) == QUEUE_ACTION_CONDITION_PULSE) {
            connectConditioner(pairedBattery, DISCHARGE_CONDITION_PULSE);
            advanceQueue(pairedBattery);
        }
        else if (queueGetNext(pairedBattery) == QUEUE_ACTION_TEST_QUICK) {
            connectConditioner(pairedBattery, DISCHARGE_TEST_QUICK);
            advanceQueue(pairedBattery);
        }
        else if (queueGetNext(pairedBattery) == QUEUE_ACTION_TEST_HIGH_RATE) {
            connectConditioner(pairedBattery, DISCHARGE_TEST_HIGH_RATE, false);
            advanceQueue(pairedBattery);
        }
        else if (queueGetNext(pairedBattery) == QUEUE_ACTION_CONDITION_LOW) {
            connectConditioner(pairedBattery, DISCHARGE_CONDITION_LOW);
            advanceQueue(pairedBattery);
        }
        clearQueue(battID);
    }
    else if (event == EVENT_BATTERY_TEST_END) {
        queue_action nextQueue = queueGetNext(battID);
        uint8_t pairedBattery = 0;

        switch (battID) {
            case 0: {
                pairedBattery = 1;
                break;
            }
            case 1: {
                pairedBattery = 0;
                break;
            }
            case 2: {
                pairedBattery = 3;
                break;
            }
            case 3: {
                pairedBattery = 2;
                break;
            }
        }

        if (queueGetNext(pairedBattery) == QUEUE_ACTION_CONDITION_PULSE) {
            connectConditioner(pairedBattery, DISCHARGE_CONDITION_PULSE);
            advanceQueue(pairedBattery);
            debugln((char*)"connecting cond");
        }
        else if (queueGetNext(pairedBattery) == QUEUE_ACTION_TEST_QUICK) {
            connectConditioner(pairedBattery, DISCHARGE_TEST_QUICK);
            advanceQueue(pairedBattery);
            debugln((char*)"starting quick test");
        }
        else if (queueGetNext(pairedBattery) == QUEUE_ACTION_CONDITION_LOW) {
            connectConditioner(pairedBattery, DISCHARGE_CONDITION_LOW);
            advanceQueue(pairedBattery);
            debugln((char*)"starting quick test");
        }
        else if (nextQueue == QUEUE_ACTION_CONDITION_PULSE) {
            connectConditioner(battID, DISCHARGE_CONDITION_PULSE);
            advanceQueue(battID);
        }
        else if (nextQueue == QUEUE_ACTION_TEST_QUICK) {
            connectConditioner(battID, DISCHARGE_TEST_QUICK);
            advanceQueue(battID);
        }
        else if (nextQueue == QUEUE_ACTION_CONDITION_LOW) {
            connectConditioner(battID, DISCHARGE_CONDITION_LOW);
            advanceQueue(battID);
        }
        if (nextQueue == QUEUE_ACTION_CHARGE) {
            connectCharger(battID);
            advanceQueue(battID);
        }
    }

    if (event == EVENT_CONDITIONER_START || event == EVENT_CONDITIONER_END || event == EVENT_CONDITIONER_HALT || event == EVENT_CONDITIONER_SWAP_STAGE || event == EVENT_BATTERY_TEST_END) {
        refreshMenu();
    }
}

void Menu::clearResults() {
    for (uint8_t i = 0; i < NUM_CHARGING_CIRCUITS; i++) {
        wattHourReadings[i] = 0.0;
        highRateTestResults[i] = 0.0;
        quickTestResults[i] = QUICK_TEST_STATUS_NOT_TESTED;
    }
}

bool Menu::connectCharger(uint8_t id) {
    return chargers[id]->connect();
}

bool Menu::disconnectCharger(uint8_t id) {
    debugln("Disconnecting charger");
    return chargers[id]->disconnect();
}

bool Menu::connectConditioner(uint8_t battID, discharge_type type, bool waitForTest) {
    Conditioner *cond = (battID > 1) ? conditioners[1] : conditioners[0];
    if (cond->connect(*batteries[battID])) {
        if (!cond->start(type, waitForTest)) {
            cond->disconnect(false);
            Serial.println("could not start!");
            return false;
        }
    }
    else {
        Serial.println("Could not connect!");
        return false;
    }
    return true;
}

bool Menu::disconnectConditioner(uint8_t battID) {
    uint8_t condID = (battID > 1) ? 1 : 0;
    debugln("Canceling conditioner");
    return conditioners[condID]->disconnect(true);
}

bool Menu::queueIsEmpty(uint8_t battID) {
    if (queueGetNext(battID) == QUEUE_ACTION_NONE)
        return true;
    else
        return false;
}

bool Menu::queueAction(uint8_t battID, queue_action action) {
    if (action != QUEUE_ACTION_NONE) {
        //if the battery is idle and there is nothing in the queue it will just perform the inputted action
        if (batteries[battID]->getState() == BATTERY_IDLE && actionQueue[battID][actionQueueStart[battID]] == QUEUE_ACTION_NONE) {
            debugln("queue empty and batt idle!");
            switch (action) {
                case QUEUE_ACTION_CHARGE: {
                    connectCharger(battID);
                    return true;
                    break;
                }
                case QUEUE_ACTION_CONDITION_PULSE: {
                    if (connectConditioner(battID, DISCHARGE_CONDITION_PULSE)) {
                        return true;
                    }
                    break;
                }
                case QUEUE_ACTION_CONDITION_LOW: {
                    if (connectConditioner(battID, DISCHARGE_CONDITION_LOW)) {
                        return true;
                    }
                    break;
                }
                case QUEUE_ACTION_TEST_QUICK: {
                    if (connectConditioner(battID, DISCHARGE_TEST_QUICK)) {
                        return true;
                    }
                    break;
                }
                case QUEUE_ACTION_TEST_HIGH_RATE: {
                    if (connectConditioner(battID, DISCHARGE_TEST_HIGH_RATE, false)) {
                        return true;
                    }
                    break;
                }
                case QUEUE_ACTION_NONE: {
                    return true;
                    break;
                }
            }
        }
        uint8_t tmp = 0;
        for (uint8_t i = actionQueueStart[battID]; i < QUEUE_MAX_DEPTH; i++) {
            tmp = (actionQueueStart[battID] + i) % QUEUE_MAX_DEPTH;
            if (actionQueue[battID][tmp] == QUEUE_ACTION_NONE) {
                actionQueue[battID][tmp] = action;
                return true;
                break;
            }
        }
    }
    debugln("could not find space for action in queue!");
    return false;
}

queue_action Menu::popQueue(uint8_t battID) {
    queue_action action = actionQueue[battID][actionQueueStart[battID]];
    actionQueue[battID][actionQueueStart[battID]] = QUEUE_ACTION_NONE;
    if (action != QUEUE_ACTION_NONE)
        actionQueueStart[battID] = (actionQueueStart[battID] + 1) % QUEUE_MAX_DEPTH;
    return action;
}

queue_action Menu::queueGetNext(uint8_t battID) {
    return actionQueue[battID][actionQueueStart[battID]];
}

void Menu::advanceQueue(uint8_t battID) {
    actionQueue[battID][actionQueueStart[battID]] = QUEUE_ACTION_NONE;
    actionQueueStart[battID] = (actionQueueStart[battID] + 1) % QUEUE_MAX_DEPTH;
}

void Menu::clearQueue(uint8_t battID) {
    for (uint8_t i = 0; i < QUEUE_MAX_DEPTH; i++) {
        actionQueue[battID][i] = QUEUE_ACTION_NONE;
    }
    actionQueueStart[battID] = 0;
}

bool Menu::chargeIsInQueue(uint8_t battID) {
    if (batteries[battID]->getState() == BATTERY_CHARGING)
        return true;

    for (uint8_t i = 0; i < QUEUE_MAX_DEPTH; i++) {
        if (actionQueue[battID][i] == QUEUE_ACTION_CHARGE)
            return true;
    }
    return false;
}
